#include "transitHub.h"
#include <sys/time.h>
#include <fstream>
#include <sstream>
#include <string>
#include <iostream>
#include <iomanip>
#include <unistd.h>
#include <cmath>
#include "readcfg.h"
#include "sendmsg.h"
#include "MotionDataInterface.h"
#include <sstream>
using namespace std;
using namespace MOTION;
namespace MOTION
{
double sampletime = 0;
}

#include<sys/ioctl.h>
#include<fcntl.h>
#include<termios.h>
#include<string.h>




ofstream errorLogTH("./debugLogs/errorLogTH.txt");
ofstream dataReceived("./debugLogs/dataReceived.txt");
ofstream Motiondata("./debugLogs/Motiondata.txt");
ofstream compassLog("./debugLogs/compass.log");
ofstream batteryLog("./debugLogs/battery.log");
ofstream gyroLog("./debugLogs/gyroscope.log");
ofstream timeinterval("./mc2sc_uart_send_timeinterval");
ofstream recievelog("./recieve.log");
ofstream gaitlog("./gait.log");
struct timeval myclock[2];

struct config* cfg;
transitHub::transitHub():bOpen(false),m_gaitCycle(0)
{

    fd = -1;
    cfg=cfg_load_file("param.cfg");
    pthread_mutex_init(&m_lock,NULL);
    // Revised for GS, 2013-04-01
    // Revision ends.
    for(int i=0;i<20;i++)
        simulator_motins[i]=0;
    ros_simular=cfg_getnum(cfg,"ros_simular");
    m_imu_version = "ADIS16365AMLZ";

    string m=cfg_getstr(cfg,"motor");
    istringstream istr(m);
    int i=0;
    while(istr>>initmotor[i++])
        cout<<initmotor[i-1]<<" ";
    cout<<endl;
    if(ros_simular)
        connect_init();
   // cout<<i;
   m_gait=new gaitStep("auto",100);
   recordGait=0;
}

transitHub::~transitHub()
{
    pthread_mutex_destroy(&m_lock);
    delete m_gait;
    portClose();
}

transitHub *transitHub::getInstance() throw(std::bad_alloc)
{
    static std::auto_ptr<transitHub> sp(new transitHub);
    static bad_alloc alloc_failed;
    if(sp.get() == NULL)
        throw(alloc_failed);
    return sp.get();
}

bool transitHub::portOpen(int comNumber, int baudRate)
{
    if(ros_simular)
    {

    return true;
    }
    portClose();
    stringstream conv;
    string numberStr;
    conv<<comNumber;
    conv>>numberStr;

    ::sleep(1);
    struct termios newtio;
    string devStr = string("/dev/ttyS")+(numberStr);
    fd = open(devStr.c_str(), O_RDWR | O_NOCTTY );//|O_NONBLOCK);
    if (fd <0)
    {
        bOpen = false;

       // LOG(ERROR, {cout<<getGaitTime()<<" Com created error! Occur in construction function \"open\""<<endl;})

        return false;
    }
    else
        bOpen =true;
    bzero(&newtio, sizeof(newtio));
    if(57600 == baudRate)
    {
        newtio.c_cflag = B57600| CS8 | CLOCAL | CREAD;
    }
    else if(115200 == baudRate)
    {
        newtio.c_cflag = B115200| CS8 | CLOCAL | CREAD;
    }
    else
    {
        newtio.c_cflag = B57600| CS8 | CLOCAL | CREAD;
    }
    newtio.c_iflag = 0;
    newtio.c_oflag = 0;
    newtio.c_lflag = 0;
    newtio.c_cc[VTIME] = 0;
    newtio.c_cc[VMIN] = 0;
    tcflush(fd, TCIOFLUSH);
    tcsetattr(fd,TCSANOW,&newtio);
    return true;
}
bool transitHub::isOpen()
{
    return bOpen;
}
bool transitHub::portClose()
{
    if(isOpen())
#ifdef LINUX
        close(fd);
#else
        CloseHandle(hCom);
#endif
    bOpen = false;
    return true;
}
int transitHub::transmit(int *data,packet_types type)
{

    char str[200];
    int length = makeStdData(str,data,type);
    // tcflush(fd,TCIOFLUSH);/////////////  modified by cxy
    int leftBytes;
    ioctl(fd,FIONREAD,&leftBytes);
    if(leftBytes>0)
    {
        char *temp = new char[leftBytes+1];
        receive(temp,leftBytes);

        delete []temp;
    }

    //////////////////////////////////
    //NON_BLOCK
//    int i = 0,j;
//    while((j=write(fd, str+i, length-i))<length-i)
//    {
//        j=j<0?0:j;
//        i+=j;
//    }
//    int nwrite = length;
    /////////////////////////////////

    int nwrite = write(fd, str, length);
    /*
         * \brief The following is used by cxy to get every time interval and sent size
         *        from main-controller to sub-controller.
    */
    static double count = 1;
    gettimeofday(&myclock[1], NULL);
    if( count++ != 1)
    {
        timeinterval<<count<<"   "<<nwrite<<"   "<<1000000 * (myclock[1].tv_sec - myclock[0].tv_sec)
        + myclock[1].tv_usec - myclock[0].tv_usec<<endl;   ///< temp add by cxy to get the send time interval
    }

    myclock[0]=myclock[1];

    if(nwrite == -1)
    {
        cout<<getGaitTime()<<" Write SerialPort Error! Occur in function \"transmit\""<<endl;

    }
    return nwrite;
}

int transitHub::receive(char *str,int length)
{
    int actualLength;


    struct termios options;
    tcgetattr(fd, &options);
    options.c_cc[VMIN] = length;
    options.c_cc[VTIME] = 1;
    tcsetattr(fd,TCSANOW,&options);
    actualLength = read(fd, str, length);
    str[actualLength] = '\0';

    return actualLength;
}

int transitHub::makeStdData(char *str,int *data,packet_types type)
{
    int len=0;
    str[len++] = (char)(0xff);				//start with "0xff 0xff"
    str[len++] = (char)(0xff);
    str[len++] = type;						//the type, i.e. 0x01,0x02,0x03... see more in pc_mega_com_protocol
    if(type == PACKET_MOTION) 			//1.PACKET_MOTION default
    {
        initdataDebug initdata;
        //initdata = m_status->getMotorinit(); 这句话导致舵机的中值点在由512 变为0；
        int absData[MOTORNUM];
        for(int i=0; i<MOTORNUM; i++)
            absData[i] = initdata.initial[i] + data[i];
        char sum = 0;
        //3rd data is reserved
        str[len++] = 2;				//
        sum += str[len-1];
        for(int i=0; i<MOTORNUM; i++)
        {
            //absData[0,2^10]
            str[len++]=(absData[i]>>8) & 0x00ff;
            str[len++]=absData[i] & 0x00ff;
            sum += str[len-1] + str[len-2];
        }
        str[len++] = (~sum & 0x00ff);//校验码？
        str[len] = '\0';
        return len;
    }
    else if(type == PACKET_PING)			//2.PACKET_PING
    {
        str[len] = '\0';
        return len;
    }
    else if(type == PACKET_CHECK)			//3.PACKET_CHECK
    {
        str[len++] = data[0] & 0x00ff;
        str[len++] = data[1] & 0x00ff;
        str[len] = '\0';
        return len;
    }
    else if(type == PACKET_SET_INIT)		//4.PACKET_SET_INIT
    {
        char sum = 0;
        str[len++] = data[0] & 0x00ff;
        str[len++] = (data[1]>>8) & 0x00ff;
        str[len++] = data[1] & 0x00ff;
        sum += str[len-1] + str[len-2] + str[len-3];
        str[len++] = (~sum & 0x00ff);
        str[len] = '\0';
        return len;
    }
    else if (type == PACKET_GYRO_PRESS)		//5.PACKET_GYRO_PRESS
    {
        str[len++] = data[0] & 0x00ff;
        str[len++] = data[1] & 0x00ff;
        str[len] = '\0';
        return len;
    }
    else if(type == PACKET_LOCK)			//6.PACKET_LOCK
    {
        str[len++] = data[0] & 0x00ff;
        str[len++] = data[1] & 0x00ff;
        str[len] = '\0';
        return len;
    }
    return 0;
}

void transitHub::doRx(char *recv /* = NULL */,int failureCount /* = 0 */)
{
    char recBuffer[200];
    char type;
    int count = failureCount;
    //while(failureCount==0?1:count--) do onece
    for(int i=0;i<1;i++)
    {
        //step-1//head////////////////////////////////////
        receive(recBuffer,1);
        if(recBuffer[0] != (char)(0xee))
        {
            continue;
        }
        receive(recBuffer,1);
        if(recBuffer[0] != (char)(0xee))
        {
            continue;
        }
        //step-2//type//////////////////////////////////////
        receive(recBuffer,1);
        type = recBuffer[0];

        recievelog<<"type:"<<type<<"\n";
        //step-3//gyro////////////////////////////////////
        if((type & 0x10) != 0)
        {
            char checksum = 0;
            receive(recBuffer,13);
            for(int i=0; i<13; i++)
            {
                checksum += recBuffer[i];
            }
            if((checksum&0xff) != 0xff)
            {
//                LOG(ERROR, {cout<<getGaitTime()<<" gyro checksum error! Occur in function \"doRx\""<<endl;})
                continue;
            }
           gypdatatransform(recBuffer);
        }
        //step-4//press/////////////////////////////////
        if((type & 0x20) != 0)
        {
            char checksum = 0;
            receive(recBuffer,17);
            for(int i=0; i<17; i++)
            {
                checksum += recBuffer[i];
            }
            if((checksum&0x00ff) != 0x00ff)
            {
                //LOG(ERROR, {cout<<getGaitTime()<<" press checksum error! Occur in function \"doRx\""<<endl;})

                continue;
            }
//            feetdatatransform(recBuffer);
        }
        //step-5 compass
        if((type & 0x40) != 0)
         {//电子罗盘
                    char checksum = 0;
                    int len = 9;
                    receive(recBuffer,len);
                    for(int i=0; i<len; i++)
                    {
                        checksum += recBuffer[i];
                    }
                    if((checksum&0xff) != 0xff)
                    {
                        cout<<getGaitTime()<<" compass checksum error! Occur in function \"doRx\""<<endl;
                        continue;
                    }
        }
        //step-6 Battery Voltage
        if((type&0x80) !=0)
        {
            char checksum = 0;

            receive(recBuffer,3);
        }


        //step-7 //motor pos////////////////////////////////////
        if((type & 0x01) != 0)
        {
            char checksum = 0;
            receive(recBuffer,1);
            checksum += recBuffer[0];
            int len = static_cast<unsigned char>(recBuffer[0]) - 1;
            receive(recBuffer,len);
            for(int i=0; i<len; i++)
            {
                checksum += recBuffer[i];
            }
            if((checksum&0x00ff) != 0x00ff)
            {

                cout<<getGaitTime()<<" motor pos checksum error! Occur in function \"doRx\""<<endl;
                continue;
            }
            else
            {
                posdataDebug motorpos;
                if(len == 4)
                    motorpos.position[recBuffer[0]-1] = (recBuffer[1]<<8)+(recBuffer[2] & 0x00ff);
                else if (len == 9)
                {
                    //record the 3 4 10 14 motor pos
                    motorpos.position[2] = (recBuffer[0]<<8)+(recBuffer[1] & 0x00ff);
                    motorpos.position[3] = (recBuffer[2]<<8)+(recBuffer[3] & 0x00ff);
                    motorpos.position[9] = (recBuffer[4]<<8)+(recBuffer[5] & 0x00ff);
                    motorpos.position[13] = (recBuffer[6]<<8)+(recBuffer[7] & 0x00ff);
                }
                else
                {
                    for(int i=0; i<(len-1)/2; i++)
                    {
                        motorpos.position[i] = (recBuffer[2*i]<<8)+(recBuffer[2*i+1] & 0x00ff);
                    }
                }
                motorpos.corresCycle = getGaitTime();
            }
        }
        //step-8//motor temp////////////////////////////////////
        if((type & 0x02) != 0)
        {
            char checksum = 0;
            receive(recBuffer,1);
            checksum += recBuffer[0];
            int len = static_cast<unsigned char>(recBuffer[0]) - 1;
            receive(recBuffer,len);
            for(int i=0; i<len; i++)
            {
                checksum += recBuffer[i];
            }
            if((checksum&0x00ff) != 0x00ff)
            {
                cout<<getGaitTime()<<" motor temp checksum error! Occur in function \"doRx\""<<endl;
                continue;
            }
            else
            {
                tempdataDebug motortemp;
                if(len == 3)
                    motortemp.temperature[recBuffer[0]-1] = recBuffer[1];
                else
                {
                    for(int i=0; i<(len-1); i++)
                    {
                        motortemp.temperature[i] = static_cast<unsigned char>(recBuffer[i]);
                    }
                }
                motortemp.corresCycle = getGaitTime();
                //m_status->setMotortemp(motortemp);
            }
        }
        //step-9//motor vol////////////////////////////////////
        if((type & 0x04) != 0)
        {
            char checksum = 0;
            receive(recBuffer,1);
            checksum += recBuffer[0];
            int len = static_cast<unsigned char>(recBuffer[0]) - 1;
            receive(recBuffer,len);
            for(int i=0; i<len; i++)
            {
                checksum += recBuffer[i];
            }
            if((checksum&0x00ff) != 0x00ff)
            {

                cout<<getGaitTime()<<"motor vol checksum error! Occur in function \"doRx\""<<endl;
                continue;
            }
            else
            {
                voldataDebug motorvol;
                if(len == 3)
                    motorvol.voltage[recBuffer[0]-1] = recBuffer[1];
                else
                {
                    for(int i=0; i<(len-1); i++)
                    {
                        motorvol.voltage[i] = static_cast<unsigned char>(recBuffer[i]);
                    }
                }
                motorvol.corresCycle = getGaitTime();
                //m_status->setMotorvol(motorvol);
            }
        }
        //step-10//motor load////////////////////////////////////
        if((type & 0x08) != 0)
        {
            char checksum = 0;
            receive(recBuffer,1);
            checksum += recBuffer[0];
            int len = static_cast<unsigned char>(recBuffer[0]) - 1;
            receive(recBuffer,len);
            for(int i=0; i<len; i++)
            {
                checksum += recBuffer[i];
            }
            if((checksum&0x00ff) != 0x00ff)
            {
                cout<<getGaitTime()<<" motor load checksum error! Occur in function \"doRx\""<<endl;
                continue;
            }
            else
            {
                loaddataDebug motorload;
                if(len == 4)
                    motorload.load[recBuffer[0]-1] = (recBuffer[1]<<8)+(recBuffer[2] & 0x00ff);
                else
                {
                    for(int i=0; i<(len-1)/2; i++)
                    {
                        motorload.load[i] = (recBuffer[2*i]<<8)+(recBuffer[2*i+1] & 0x00ff);
                    }
                }
                motorload.corresCycle = getGaitTime();
                //m_status->setMotorload(motorload);
            }
        }


        //step-11//data request + state feedback////////////////////////////
        if(recv != NULL)
        {
            receive(recBuffer,2);
            recv[0] = recBuffer[0];
            recv[1] = recBuffer[1];
        }
        break;
    }
}

void transitHub::doLoopTx(int *body /* = NULL */, int *camera /* = NULL */)
{
    if(!body)
        return;
    if(recordGait)
        m_gait->pushData(body);
    if(ros_simular)
    {

    int rate=15;
    if(!body) return ;
       bool go=false;
        do{
                int d=0;
                go=false;
               for(int i=0;i<18;i++){
               d=body[i]-simulator_motins[i];
                if(d<-rate){
                    d=-rate;go=true;
                }
                if(d>rate){
                    d=rate;go=true;
                }
                simulator_motins[i]+=d;
               }

            usleep(15*1000);

        send_msg(simulator_motins);

        }while(go);

        //cout<<"transithub simulation dotask\n";
        return ;
    }

    int store_data[MOTORNUM];//for storing the transmitted data
    char recv[3];					//for storing the received data

//    m_status->getCurrentMotorData(store_data);
    for(int i=0; i<MOTORNUM; i++) //if paras are NULL, just use the former data
    {
        if(body != NULL && i < MOTORNUM)
            store_data[i] = body[i]+initmotor[i];
    }
    int max_dmotor=0;
     for(int i=0;i<20;i++)
    {
        if(store_data[i]>1024||store_data[i]<0)
        {
            cout<<"gait beyond limits"<<i<<","<<store_data[i]<<endl;
            return;
        }
        max_dmotor=max(max_dmotor,(int)abs(simulator_motins[i]-store_data[i]));
        simulator_motins[i]=store_data[i];
    }

    transmit(store_data);
    struct timeval tv1,tv2;
    gettimeofday(&tv1, NULL);
    while(1)
    {
        //cout<<"doRX"<<endl;
        doRx(recv);
        if(recv[1]==(char)NONE)	//if lost, redo
        {
            transmit(store_data);
            cout<<getGaitTime()<<"	"<<"lost"<<endl;

            continue;
        }
        if(recv[1]==(char)FAILURE)	//if error, redo
        {
            transmit(store_data);
            cout<<"WARNING: Check the gait file with the initial motor position!"<<endl;
            continue;
        }

        while(recv[0]==(char)'O')//when the lower control board doesn't require for data, just ping
        {
            cout<<"doCheckTx\n";
            doCheckTx();
            doRx(recv);
        }
        increaseGaitTime();
        break;
    }
    gettimeofday(&tv2, NULL);
    gaitlog<<"gaite:"<<getGaitTime()<<"max_change:"<<max_dmotor<<"use time(us):"<<((tv2.tv_sec-tv1.tv_sec)*1000000+tv2.tv_usec-tv1.tv_usec)<<endl;
}
void transitHub::doCheckTx(int id /* = 0 */,bool pos /* = false */, bool temp /* = false */, bool vol /* = false */, bool load /* = false */)
{
    int data[2];
    data[0] = id;
    data[1] = (pos?0x01:0)|(temp?0x02:0)|(vol?0x04:0)|(load?0x08:0);
    if(data[0] != 0 && data[1] != 0)
    {
        transmit(data,PACKET_CHECK);
    }
    else
    {
        transmit(NULL,PACKET_PING);
    }
}
void transitHub::doWriteCommandTx(int idOrItem, int para, packet_types type)
{
    //PACKET_SET_INIT data = {2 values, i.e. 1st refers to the motor id, 2nd refers to the goal position}
    //PACKET_GYRO_PRESS: data = {2 value, i.e. 1st refers to gyro if it's 0, to press if it's 1, 2nd refers to return command if it's 1, to NOT return command if it's 0}
    //PACKET_LOCK: data = {2 values, i.e. 1st refers to the motor id, 2nd refers to lock if it's 1, to unlock if it's 0}
    int data[2];
    data[0] = idOrItem;
    data[1] = para;
    if(type == PACKET_SET_INIT || type == PACKET_GYRO_PRESS || type == PACKET_LOCK)
        transmit(data,type);
    else
        return;
}
long transitHub::getGaitTime()
{
    return m_gaitCycle;
}
void transitHub::increaseGaitTime()
{
    m_gaitCycle ++;
}
void transitHub::clearGaitTime()
{
    m_gaitCycle = 0;
}
void transitHub::gypdatatransform(char *temp)//陀螺仪
{
    timeval time_temp;

    gdataDebug tempgdata;
    unsigned short int tmp1[12];
    for(int i=0; i<12; i++)
        tmp1[i]=temp[i];
    unsigned short int tmp2[6];
    for(int i=0; i<6; i++)
        tmp2[i]=((tmp1[2*i]<<8)&0xff00)+(tmp1[2*i+1]&0x00ff);
    short int tmp[6];

    for(int i=0; i<6; i++)
        if(tmp2[i]>0xb000)
            tmp[i]=(tmp2[i]|0xc000);
        else
            tmp[i]=(tmp2[i]&0x3fff);

    if(tmp[0]==0 && tmp[1]==0 && tmp[2]==0 && tmp[3]==0 && tmp[4]==0 && tmp[5]==0)//if error, just be equal to the former
    {
        cout<<"gyp:error\n";
        return;
    }

    if(m_imu_version == "ADIS16365BMLZ")    // 陀螺仪型号为 ADIS 16365BMLZ
    {
        tempgdata.GYPO[0]=tmp[0]*0.05;
        tempgdata.GYPO[1]=tmp[1]*0.05;
        tempgdata.GYPO[2]=tmp[2]*0.05+m_gyroscope_correct;
        tempgdata.ACCL[0]=tmp[3]*0.003*9.8;
        tempgdata.ACCL[1]=tmp[4]*0.003*9.8;
        tempgdata.ACCL[2]=tmp[5]*0.003*9.8;
    }
    else if(m_imu_version == "ADIS16365AMLZ")  //陀螺仪型号为 ADIS 16355AMLZ（默认）
    {
        tempgdata.GYPO[0]=tmp[0]*0.07326;
        tempgdata.GYPO[1]=tmp[1]*0.07326;
        tempgdata.GYPO[2]=tmp[2]*0.07326+m_gyroscope_correct;
        tempgdata.ACCL[0]=tmp[3]*0.002522*9.8;
        tempgdata.ACCL[1]=tmp[4]*0.002522*9.8;
        tempgdata.ACCL[2]=tmp[5]*0.002522*9.8;
    }
    else if(m_imu_version=="MPU6000")
    {
        //gyro full range -1000dps~+1000dps
        //16bits in 2'complement and 32.8 LSB/ °/s
        tempgdata.GYPO[0]=tmp[0]*0.0304878;
        tempgdata.GYPO[1]=tmp[1]*0.0304878;
        if(m_robot_version == "2012")
        {
            tempgdata.GYPO[2]=tmp[2]*0.0304878+m_gyroscope_correct;
        }
        else // "2012.5" and "2013"
        {
//            tempgdata.GYPO[2]=-1.0*(tmp[2]*0.0304878+m_gyroscope_correct);
            tempgdata.GYPO[2]=-1.0*(tmp[2]*0.0304878);
        }
        //acc full range -8g~+8g
        //16bits in 2'complement and  4096 LSB/g
        tempgdata.ACCL[0]=tmp[3]*0.0002441*9.8;
        tempgdata.ACCL[1]=tmp[4]*0.0002441*9.8;
        tempgdata.ACCL[2]=tmp[5]*0.0002441*9.8;

    }
    else
    {
        cout<<getGaitTime()<<" error in gyro transform"<<endl;
    }

    tempgdata.corresCycle = getGaitTime();


    if(m_gaitCycle <= 3)
    {

        gettimeofday(&time_temp, NULL);
        gyro_timeval[1] = 1000000 * time_temp.tv_sec + time_temp.tv_usec;
        gyro_timeval[1] /= 1000000; // init value

        sampletime = 0.02;
    }
    else
    {
        gettimeofday(&time_temp, NULL);
        gyro_timeval[0] = gyro_timeval[1]; // set past
        gyro_timeval[1] = 1000000 * time_temp.tv_sec + time_temp.tv_usec;
        gyro_timeval[1] /= 1000000; // get new value

        sampletime = gyro_timeval[1] - gyro_timeval[0];
        if(sampletime > 0.03)
        {
            sampletime = 0.03;
        }
//        cout<<"sample time: "<<sampletime<<endl;
    }
    if(m_robot_version == "2012")
    {
        tempgdata.GYPO[2] += 0;
    }
    else // "2012.5" and "2013"
    {
//            tempgdata.GYPO[2]=-1.0*(tmp[2]*0.0304878+m_gyroscope_correct);
        tempgdata.GYPO[2] +=-1.0*m_gyroscope_correct;
    }
    char t[80];
    sprintf(t,"a/g: %.3f,%.3f,%.3f;%.3f,%.3f,%.3f\n",tempgdata.ACCL[0],tempgdata.ACCL[1],tempgdata.ACCL[2],tempgdata.GYPO[0],tempgdata.GYPO[1],tempgdata.GYPO[2]);
    recievelog<<t;
/*
    m_status->setGdata(tempgdata);
    m_status->updateEularAngle();

    m_status->checkStableState();
*/
}

